/*
 * @Description: 地图匹配任务管理， 放在类里使代码更清晰
 * @Author: Zhijian Qiao
 * @Date: 2020-02-10 08:38:42
 */
#include "matching/matching_flow.hpp"
#include "glog/logging.h"
#include "global_defination/global_defination.h"
#include "CommonFunc.h"
#include "tools/print_info.hpp"
#include "iostream"
#include "tools/se3_util.hpp"

using namespace std;

namespace avp_slam {
    MatchingFlow::MatchingFlow(ros::NodeHandle &nh) {

        YAML::Node config_node;

        config_node = YAML::LoadFile(avp_slam::MATCH_YAML_PATH);
        use_wheel = config_node["use_wheel"].as<bool>();

        // subscriber
        images_sub_ptr_ = std::make_shared<FourImagesSubscriber>(nh, 5);
        dr_sub_ptr_ = std::make_shared<OdometrySubscriber>(nh, "/dr_pred", 100);
        image_pred_sub_ptr_ = std::make_shared<OdometrySubscriber>(nh, "/images_pred", 100);

        image_odom_pub_ptr_ = std::make_shared<OdometryPublisher>(nh, "/final_odom", "/robot_1/odom", "/robot_1/odom",
                                                                  100);
        image_path_pub_ptr_ = std::make_shared<PathPublisher>(nh, "/image_path", "/robot_1/odom", 100);
        image_gt_path_pub_ptr_ = std::make_shared<PathPublisher>(nh, "/image_gt_path", "/robot_1/odom", 100);
        image_tf_pub_ptr_ = std::make_shared<TFBroadCaster>("/robot_1/odom", "/image_link");

        imuVel_odom_pub_ptr_ = std::make_shared<OdometryPublisher>(nh, "/imu_localization", "/robot_1/odom",
                                                                   "/imu_link", 100);
        latest_path_pub_ptr_ = std::make_shared<PathPublisher>(nh, "/latest_path", "/robot_1/odom", 100);
        imuVel_tf_pub_ptr_ = std::make_shared<TFBroadCaster>("/robot_1/odom", "/base_link");

        gt_listener_ = std::make_shared<tf::TransformListener>(nh);

        matching_ptr_ = std::make_shared<Matching>();

        time_valid = std::make_shared<TicToc>("valid", false);
        LOG(INFO) << "MatchingFlow init finished";
    }

    Eigen::Matrix4f MatchingFlow::TransformToMatrix(const tf::StampedTransform &transform) {
        Eigen::Matrix4f transform_matrix = Eigen::Matrix4f::Identity();

        Eigen::Vector3f tl_btol(
                transform.getOrigin().getX(),
                transform.getOrigin().getY(),
                transform.getOrigin().getZ());
        tf::Quaternion quat = transform.getRotation();
        Eigen::Quaternionf quat_eigen(quat.w(), quat.x(), quat.y(), quat.z());
//        LOG(INFO)<<"gt xyz: "<<tl_btol(0)<<" "<<tl_btol(1)<<" "<<tl_btol(2);
        transform_matrix.block<3, 3>(0, 0) = quat_eigen.toRotationMatrix();
        transform_matrix.block<3, 1>(0, 3) = tl_btol;
        return transform_matrix;
    }

    bool MatchingFlow::Run() {

        ReadData();

        while (HasData() && ros::ok()) {
            if (!ValidData()) {
                continue;
            }
            if (UpdateMatching()) {
                PublishData();
            }
        }
        return true;
    }

    bool MatchingFlow::ReadData() {
        images_sub_ptr_->ParseData(image_data_buff__);
//        预测的帧间位姿
        image_pred_sub_ptr_->ParseData(image_pred_data_buff_);
//        里程计数据序列
        dr_sub_ptr_->ParseData(dr_data_buff_);
        return true;
    }

    bool MatchingFlow::HasData() {
        if (!dr_data_buff_.empty()) {//program continue even if imu fail
            updateLatestPose();
        }

        if (image_pred_data_buff_.empty())
            return false;

        if (image_data_buff__.empty())
            return false;

        if (matching_ptr_->HasInited())
            return true;

        return true;
    }

    bool MatchingFlow::ValidData() {
        while (image_data_buff__.size() > 1)
            image_data_buff__.pop_front(); //为了实时性，只处理最后一帧图像

        current_image_data_ = image_data_buff__.front();
        image_data_buff__.pop_front();
        updatePred();

        return true;
    }

//    累乘image_pred_data_buff_获得两帧间相对位姿
    bool MatchingFlow::updatePred() {
        T_pred = Eigen::Matrix4f::Identity();
        if (!matching_ptr_->HasInited()) {
            while (!image_pred_data_buff_.empty())
                image_pred_data_buff_.pop_front();
            return true;
        } else if (!image_pred_data_buff_.empty()) {
            double last_time = 0.0;
            while (!image_pred_data_buff_.empty()) {
                PoseData image_pred = image_pred_data_buff_.front();
                double startTime = image_pred.startTime;
                double endTime = image_pred.endTime;
                if (startTime >= previous_image_data_.time && endTime <= current_image_data_.time &&
                    startTime > last_time) {
                    T_pred = T_pred * image_pred.pose;
                    image_pred_data_buff_.pop_front();
                    last_time = startTime;
//                    LOG(INFO) << "previous_image_data_ = " <<  previous_image_data_.time << ", current_image_data_ = " << current_image_data_.time << ", start = " << startTime << ", endTime = " << endTime;
                } else if (endTime <= previous_image_data_.time) {
                    image_pred_data_buff_.pop_front();
                } else
                    break;
            }
            return true;
        } else {// empty
            //除非imu停了，否则不会有这种情况，因为data_pretreat中先发dr再发图像
            return false;//但是为了实时性，ValidData不会因为这个false阻塞图像更新
        }
    }

//根据全局地图更新位姿Twb_map_
    bool MatchingFlow::UpdateMatching() {

        if (!matching_ptr_->HasInited()) {
            matching_ptr_->Initialize(Eigen::Matrix4f::Identity(), CloudData());    //设置局部地图/初始位置，init_pose_
            if (!matching_ptr_->HasInited()) {
                LOG(INFO) << "Initialize Fail, and reinitialize." << endl;
                return false;
            } else {
                Eigen::Matrix3f rpy = matching_ptr_->GetInitPose().block(0, 0, 3, 3);
                LOG(INFO) << "Initialize Success with rpy " << dcm2rpy(rpy).transpose() * 180.0 / M_PI << " degrees."
                          << endl;
            }
        }
//         利用图像去更新位姿, 完全信任里程计的预测量
        Twb_map_ = Twb_map_ * T_pred;
        previous_image_data_ = current_image_data_;
//        matching_ptr_->Update(CloudData(), T_pred);
//        Twb_map_ = matching_ptr_->GetResultPose();

        return true;
    }

    bool MatchingFlow::updateLatestPose() {

        if (!matching_ptr_->HasInited()) {
            return false;
        }
        //图像帧之前的都去掉
        while (!dr_data_buff_.empty()) {
            PoseData pose_data = dr_data_buff_.front();
            double endTime = pose_data.endTime;
            if (endTime <
                current_image_data_.time) //current_image_data在ValidData中更新，所以此时的current_image_data还是上一个循环结束(updateMatching)或者continue(data not valid, 等待imu，cur image不变)时的current_image_data
                dr_data_buff_.pop_front();
            else
                break;
        }

        std::deque<PoseData> data_buff = dr_data_buff_;

//        基于相机计算出的位姿，结合里程计信息发布最新位姿
        Eigen::Matrix4f Twb_dr = Twb_map_;
        latest_path_pub_ptr_->Reset();
        while (!data_buff.empty()) {
            PoseData pose_data = data_buff.front();
            Eigen::Matrix4f poseTmp;
            if (pose_data.startTime <= current_image_data_.time) {
                float alpha =
                        (current_image_data_.time - pose_data.startTime) / (pose_data.endTime - pose_data.startTime);
                Eigen::Matrix4f M0 = Eigen::Matrix4f::Identity();
                Eigen::Matrix4f re = interpolateSE3(M0, pose_data.pose, alpha);
                poseTmp = EigenIsoInv(re) * pose_data.pose;
            } else
                poseTmp = pose_data.pose;
            Twb_dr = Twb_dr * poseTmp;
            imuVel_tf_pub_ptr_->SendTransform(Twb_dr, pose_data.endTime);
            imuVel_odom_pub_ptr_->Publish(Twb_dr, pose_data.endTime);
            latest_path_pub_ptr_->Publish(Twb_dr, pose_data.endTime);
            data_buff.pop_front();
        }
    }

    bool MatchingFlow::PublishData() {

        static Eigen::Matrix4f last_Twb_map_ = Eigen::Matrix4f::Identity();
//        PrintInfo::PrintPose("delta_map", EigenIsoInv(last_Twb_map_)*Twb_map_);
        last_Twb_map_ = Twb_map_;
//        PrintInfo::PrintPose("delta_dr", T_pred);
//        if(T_pred == Eigen::Matrix4f::Identity())
//            LOG(INFO) << "the above T_pred = Identity";
//计算出来的相机位姿
        image_tf_pub_ptr_->SendTransform(Twb_map_, current_image_data_.time);
        image_odom_pub_ptr_->Publish(Twb_map_, current_image_data_.time);
        image_path_pub_ptr_->Publish(Twb_map_, current_image_data_.time);

        //创建一个StampedTransform对象存储变换结果数据
        tf::StampedTransform transform_stamp;
        tf::Transform tfBaseToLink1;
        //监听包装在一个try-catch块中以捕获可能的异常
        try {
            //向侦听器查询特定的转换，(想得到/robot_1/map/robot_1/base_footprint)，想要转换的时间ros::Time(0)提供了最新的可用转换。
            gt_listener_->lookupTransform("/robot_1/odom", "/robot_1/base_footprint",
                                          ros::Time(0), transform_stamp);
        }
        catch (tf::TransformException &ex) {
            ROS_ERROR("%s", ex.what());
            ros::Duration(1.0).sleep();
        }
        Eigen::Matrix4f T_gt = TransformToMatrix(transform_stamp);
        image_gt_path_pub_ptr_->Publish(T_gt, transform_stamp.stamp_.toSec());
//        current_image_data_
        return true;
    }
}


// 红色为x轴，绿色为y轴，蓝色为z轴